//using namespace cv;

#include "cv.h"
#include "highgui.h"

#include "ros/ros.h"
#include <std_msgs/Int32.h>
#include <std_msgs/Int8MultiArray.h>

//#include <core.hpp>
#include <iostream>

/*
#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/image_encodings.h>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>

#include "robo/CameraParams.h"
*/
#include <string.h>
#include <signal.h>

#define IMAGEWIDTH 320
#define IMAGEHEIGHT 240
using namespace std;

int counter = 0;

void cam0_cb(const std_msgs::Int8MultiArray::ConstPtr& array)
{
	printf("cam0_cb\n");
	IplImage *img 				= cvCreateImage(cvSize(IMAGEWIDTH, IMAGEHEIGHT), IPL_DEPTH_8U, 3);
	char * data 				= img->imageData;
	for(int i = 0; i < IMAGEWIDTH*IMAGEHEIGHT*3; i++)
	{
		data[i] = char(array->data.at(i));
	}
	char filename [50];
	sprintf(filename, "test_%i_cam0.jpg" ,counter++);
	cvSaveImage(filename , img);
	cvNamedWindow("Stream_cam0", CV_WINDOW_AUTOSIZE);
	cvShowImage("Stream_cam0", img);
	cvWaitKey(2);
  cvReleaseImage(&img);
}

void cam1_cb(const std_msgs::Int8MultiArray::ConstPtr& array)
{
	printf("cam1_cb\n");
	
	IplImage *img 				= cvCreateImage(cvSize(IMAGEWIDTH, IMAGEHEIGHT), IPL_DEPTH_8U, 3);
	char * data 				= img->imageData;
	
	for(int i = 0; i < IMAGEWIDTH*IMAGEHEIGHT*3; i++)
	{
		data[i] = char(array->data.at(i));
	}
	char filename [50];
	sprintf(filename, "test_%i_cam1.jpg" ,counter++);
	cvSaveImage(filename , img);
	cvNamedWindow("Stream_cam1", CV_WINDOW_AUTOSIZE);
	cvShowImage("Stream_cam1", img);
	cvWaitKey(2);
    cvReleaseImage(&img);
}

int main(int argc, char **argv)
{
	printf("Starting VideoStream\n");
	ros::init(argc, argv, "VideoStream");
	ros::NodeHandle n;
	ros::Subscriber img0_sub = n.subscribe("/camera0_img", 1, cam0_cb);
	ros::Subscriber img1_sub = n.subscribe("/camera1_img", 1, cam1_cb);
	ros::spin();
}
